#include "animator.h"

Animator::Animator() {
   skeleton = 0;
   skeletonIni = 0;
   motion = 0;
}

Animator::Animator(Skeleton *s, Motion *m) {
   skeleton = s;
   skeletonIni = new Skeleton(*s);
   motion = m;
   currentFrame = 1;
}

Animator::~Animator() {
   if (skeletonIni != 0) delete skeletonIni;
}

void Animator::prev(){
   if (currentFrame > 1) {      
      currentFrame = currentFrame - 1;
   }
   return frame(currentFrame);
}

void Animator::next(){
   if (currentFrame < motion->getMotionSize()) {
      currentFrame = currentFrame + 1;
   }
   return frame(currentFrame);
}

void Animator::first(){
   currentFrame = 1;
   return frame(currentFrame);
}

void Animator::last(){
   currentFrame = motion->getMotionSize();
   return frame(currentFrame);
}

void Animator::frame(int f) {
   //~ cout<<"frame: "<<f<<endl;
   // s will be the original skeleton
   Skeleton s(*skeletonIni);
   
   // dont modify the original skeleton s
   if (f == 0) return;
   
   // apply the frame i into original skeleton s
   const PoseTransformations &pt = motion->getFrame(f);
   const string &rootTrOrder = s.getTrOrder();
   
   bool isDeg = false;
   if (motion->isDegree()) {
      isDeg = true;
   }
   
   Point3D rootPosition = s.getRootPosition();
   
   PoseTransformations::const_iterator pti = pt.begin();
   while (pti != pt.end()) {
      
      string currentBone = pti->first;
      string order;
      
      if (currentBone == "root") {
         order = rootTrOrder;
      } else {
         order = s.getBone(currentBone).getAxisOrder();
      }
      
      Number alfa, beta, gama, tx, ty, tz;
      alfa = beta = gama = 0.0;
      
      for (int i = 0; i < pti->second.size(); i++) {
         switch (order[i]) {
            case 'x': tx = pti->second[i]; break;
            case 'y': ty = pti->second[i]; break;
            case 'z': tz = pti->second[i]; break;
            case 'X': alfa = pti->second[i]; break;
            case 'Y': beta = pti->second[i]; break;
            case 'Z': gama = pti->second[i]; break;
         }         
      }
      
      Vector3D angles(alfa, beta, gama);
      Vector3D translation(tx, ty, tz);
      
      if (currentBone == "root") {
         s.setRootPosition(rootPosition + translation);
         Sons sons = s.getSons("root");
         for (int i = 0; i < sons.size(); i++) {
            s.rotateBone(sons[i], angles, order, true, isDeg);
         }
      } else {
         s.rotateBone(currentBone, angles, order, false, isDeg);
      }
      
      if (currentBone != "root") {
         /// rotate bones using quaternions
         const Vector3 &B = skeletonIni->getBone(currentBone).getDirection();
         Quat qB(B[0],B[1],B[2]);
         const Quat &Q = s.rotation[currentBone];
         Quat qb = Q*qB*conjugate(Q);
         Vector3 b(qb[1],qb[2],qb[3]);
         s.bones[currentBone].setDirection(b);
         
         //~ Vector3 X = skeletonIni->getBone(currentBone).getAxisX();
         //~ Vector3 Y = skeletonIni->getBone(currentBone).getAxisY();
         //~ Vector3 Z = skeletonIni->getBone(currentBone).getAxisZ();
         //~ Quat qX(X[0],X[1],X[2]);
         //~ Quat qY(Y[0],Y[1],Y[2]);
         //~ Quat qZ(Z[0],Z[1],Z[2]);
         //~ Quat qx = Q*qX*conjugate(Q);
         //~ Quat qy = Q*qY*conjugate(Q);
         //~ Quat qz = Q*qZ*conjugate(Q);
         //~ Vector3 x(qx[1],qx[2],qx[3]);
         //~ Vector3 y(qy[1],qy[2],qy[3]);
         //~ Vector3 z(qz[1],qz[2],qz[3]);
         //~ s.bones[currentBone].setAxisX(x);
         //~ s.bones[currentBone].setAxisX(y);
         //~ s.bones[currentBone].setAxisX(z);
      }
      pti++;
   }
   
   // update the current skeleton
   skeleton->setRootPosition(s.getRootPosition());
   skeleton->setBones(s.getBones());
   skeleton->rotation = s.rotation;
   
   //~ for (int i=0; i<s.nBones(); ++i) {
      //~ cout<<"   >>> "<<i<<" "<<skeleton->boneName(i)<<" : "
          //~ <<skeleton->getBoneQuaternion(i)<<" : "<<length(skeleton->getBoneQuaternion(i))<<" : "
          //~ <<skeleton->getBoneTranslation(i)<<endl;
   //~ }
   
   /// TODO: modify this
   //~ skeletons.push_back(s);
}
